﻿/*
 * ©2009-2012 XELF
 * http://xelf.info
 */

using System;

#if XNA || WINDOWS_PHONE || SILVERLIGHT
using Microsoft.Xna.Framework;
using MathHelper_ = Microsoft.Xna.Framework.MathHelper;
#else
using MathHelper_ = XELF.Framework.MathHelper;
#endif

namespace XELF.Framework {

#if XNA || WINDOWS_PHONE || SILVERLIGHT
	public static partial class MatrixHelper {
#else
	public static partial class Matrix {
#endif
		public static void CreateEulerAngleXYZ(ref Matrix value, out Vector3 result) {
			CreateEulerAngle(value.M12, value.M11, value.M13, value.M23, value.M33, -value.M21, value.M22,
				out result.X, out result.Y, out result.Z);
		}
		public static void CreateEulerAngleXZY(ref Matrix value, out Vector3 result) {
			CreateEulerAngle(value.M13, value.M11, value.M12, value.M32, value.M22, +value.M31, value.M33,
				out result.X, out result.Z, out result.Y);
			result.X = -result.X;
			result.Y = -result.Y;
			result.Z = -result.Z;
		}
		public static void CreateEulerAngleYXZ(ref Matrix value, out Vector3 result) {
			CreateEulerAngle(value.M21, value.M22, value.M23, value.M13, value.M33, +value.M12, value.M11,
				out result.Y, out result.X, out result.Z);
			result.X = -result.X;
			result.Y = -result.Y;
			result.Z = -result.Z;
		}
		public static void CreateEulerAngleYZX(ref Matrix value, out Vector3 result) {
			CreateEulerAngle(value.M23, value.M22, value.M21, value.M31, value.M11, -value.M32, value.M33,
				out result.Y, out result.Z, out result.X);
		}
		public static void CreateEulerAngleZXY(ref Matrix value, out Vector3 result) {
			CreateEulerAngle(value.M31, value.M33, value.M32, value.M12, value.M22, -value.M13, value.M11,
				out result.Z, out result.X, out result.Y);
		}
		public static void CreateEulerAngleZYX(ref Matrix value, out Vector3 result) {
			CreateEulerAngle(value.M32, value.M33, value.M31, value.M21, value.M11, +value.M23, value.M22,
				out result.Z, out result.Y, out result.X);
			result.X = -result.X;
			result.Y = -result.Y;
			result.Z = -result.Z;
		}
		/// <summary>
		/// 
		/// </summary>
		/// <param name="v0"></param>
		/// <param name="v1"></param>
		/// <param name="v2"></param>
		/// <param name="v3"></param>
		/// <param name="v4"></param>
		/// <param name="v5"></param>
		/// <param name="v6"></param>
		/// <param name="a">[-π,+π]</param>
		/// <param name="b">[-π/2,+π/2]</param>
		/// <param name="c">[-π,+π]</param>
		static void CreateEulerAngle(float v0, float v1, float v2, float v3, float v4, float v5, float v6,
			out float a, out float b, out float c) {
			b = -(float)Math.Asin(MathHelper_.Clamp(v2, -1, 1));
			var cosine = (float)Math.Cos(b);
			if (Math.Abs(cosine) < 1e-5f) {
				c = (float)Math.Atan2(v5, v6);
				a = 0;
			} else {
				c = (float)Math.Atan2(v0, v1);
				a = (float)Math.Atan2(v3, v4);
			}
		}
	}

}
